/*
Copyright (C) 2021-2025 Casa Xu (also Zhiyan Xu) from HIT

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

/*
 * LOS_MEMS.c
 *
 *  Created on: 2023年3月27日
 *      Author: 64435
 */

#include "LOS_MEMS.h"

uint8_t buf = 0xCE;

void LOS_MEMS_GetStaDyn()
{
    LOS_CAN_SendPack(LOS_CAN_NUM1, 0xCE, &buf, 1);
}

void LOS_CAN_MEMSCallBackDyn(LOS_CANMSG_t* can_re)
{
    uint8_t id_filter[4];
    int16_t gyro = 0;
    memcpy(id_filter, &(can_re->ID), 4);   //将帧ID分为4个字节存储，便于处理
    if (id_filter[3]==0xFF)                //若为首帧
    {
        memcpy(&gyro, can_re->msg, 2);
        STA_MEMS.gyro_x = ((float)gyro/200.0)*3.1415926/180.0;
        memcpy(&gyro, &can_re->msg[2], 2);
        STA_MEMS.gyro_y = ((float)gyro/200.0)*3.1415926/180.0;
        memcpy(&gyro, &can_re->msg[4], 2);
        STA_MEMS.gyro_z = ((float)gyro/200.0)*3.1415926/180.0;
        STA_MEMS.TMCNT++;
    }
}

void LOS_MEMS_BurstRead()
{
    float fbuf = 0;
    int16_t ibuf = 0;
    LOS_SPI_WriteReadByte(LOS_SPI_NUM5, 0x3E00, CS_2);
    LOS_SPI_WriteReadByte(LOS_SPI_NUM5, 0xFFFF, CS_2);  //DIAG_STAT
    STA_MEMS.odata[0] = LOS_SPI_WriteReadByte(LOS_SPI_NUM5, 0xFFFF, CS_2);  //X_GYRO_OUT
    memcpy(&ibuf,&STA_MEMS.odata[0],2);
    fbuf = ibuf;
    STA_MEMS.gyro_x = fbuf*0.005*3.1415926/180.0;
    STA_MEMS.odata[1] = LOS_SPI_WriteReadByte(LOS_SPI_NUM5, 0xFFFF, CS_2);  //Y_GYRO_OUT
    memcpy(&ibuf,&STA_MEMS.odata[1],2);
    fbuf = ibuf;
    STA_MEMS.gyro_y = fbuf*0.005*3.1415926/180.0;
    STA_MEMS.odata[2] = LOS_SPI_WriteReadByte(LOS_SPI_NUM5, 0xFFFF, CS_2);  //Z_GYRO_OUT
    memcpy(&ibuf,&STA_MEMS.odata[2],2);
    fbuf = ibuf;
    STA_MEMS.gyro_z = fbuf*0.005*3.1415926/180.0;
    STA_MEMS.odata[3] = LOS_SPI_WriteReadByte(LOS_SPI_NUM5, 0xFFFF, CS_2);  //X_ACCL_OUT
    memcpy(&ibuf,&STA_MEMS.odata[3],2);
    fbuf = ibuf;
    STA_MEMS.acc_x = fbuf*0.25*9.8/1000;
    STA_MEMS.odata[4] = LOS_SPI_WriteReadByte(LOS_SPI_NUM5, 0xFFFF, CS_2);  //Y_ACCL_OUT
    memcpy(&ibuf,&STA_MEMS.odata[4],2);
    fbuf = ibuf;
    STA_MEMS.acc_y = fbuf*0.25*9.8/1000;
    STA_MEMS.odata[5] = LOS_SPI_WriteReadByte(LOS_SPI_NUM5, 0xFFFF, CS_2);  //Z_ACCL_OUT
    memcpy(&ibuf,&STA_MEMS.odata[5],2);
    fbuf = ibuf;
    STA_MEMS.acc_z = fbuf*0.25*9.8/1000;
    LOS_SPI_WriteReadByte(LOS_SPI_NUM5, 0xFFFF, CS_2);  //TEMP_OUT
    LOS_SPI_WriteReadByte(LOS_SPI_NUM5, 0xFFFF, CS_2);  //SMPL_CNTR
    LOS_SPI_WriteReadLastByte(LOS_SPI_NUM5, 0xFFFF, CS_2);  //CHECKSUM
}


void LOS_MEMS_GetGyro()
{
    // LOS_SPI_MEMS(0X0600);
    // STA_MEMS.odata[0] = LOS_SPI_MEMS(0X0A00);
    // STA_MEMS.odata[1] = LOS_SPI_MEMS(0X0E00);
    // STA_MEMS.odata[2] = LOS_SPI_MEMS(0X1200);
    // STA_MEMS.odata[3] = LOS_SPI_MEMS(0X1600);
    // STA_MEMS.odata[4] = LOS_SPI_MEMS(0X1A00);
    // STA_MEMS.odata[5] = LOS_SPI_MEMS(0XFFFF);
    // STA_MEMS.gyro_x = (float)STA_MEMS.odata[0]*0.005*3.1415926/180.0;
    // STA_MEMS.gyro_y = (float)STA_MEMS.odata[1]*0.005*3.1415926/180.0;
    // STA_MEMS.gyro_z = (float)STA_MEMS.odata[2]*0.005*3.1415926/180.0;
    // STA_MEMS.acc_x = (float)STA_MEMS.odata[3]*0.25*9.8/1000;
    // STA_MEMS.acc_y = (float)STA_MEMS.odata[4]*0.25*9.8/1000;
    // STA_MEMS.acc_z = (float)STA_MEMS.odata[5]*0.25*9.8/1000;
    LOS_MEMS_Getgyrox();
    LOS_MEMS_Getgyroy();
    LOS_MEMS_Getgyroz();
    LOS_MEMS_Getaccx();
    LOS_MEMS_Getaccy();
    LOS_MEMS_Getaccz();
}

void LOS_MEMS_Getgyrox()
{
    float buf = 0;
    // STA_MEMS.odata[0] = LOS_SPI_MEMS(0X0600);
    // STA_MEMS.odata[0] = LOS_SPI_MEMS(0X0600);
    // STA_MEMS.odata[0] = LOS_SPI_MEMS(0X0600);
    LOS_SPI_MEMS(0X0600);
    STA_MEMS.odata[0] = LOS_SPI_MEMS(0X0600);
    buf = STA_MEMS.odata[0];
    STA_MEMS.gyro_x = buf*0.005*3.1415926/180.0;
}

void LOS_MEMS_Getgyroy()
{
    float buf = 0;
    // STA_MEMS.odata[1] = LOS_SPI_MEMS(0X0A00);
    // STA_MEMS.odata[1] = LOS_SPI_MEMS(0X0A00);
    // STA_MEMS.odata[1] = LOS_SPI_MEMS(0X0A00);
    LOS_SPI_MEMS(0X0A00);
    STA_MEMS.odata[1] = LOS_SPI_MEMS(0X0A00);
    buf = STA_MEMS.odata[1];
    STA_MEMS.gyro_y = buf*0.005*3.1415926/180.0;
}

void LOS_MEMS_Getgyroz()
{
    float buf = 0;
    // STA_MEMS.odata[2] = LOS_SPI_MEMS(0X0E00);
    // STA_MEMS.odata[2] = LOS_SPI_MEMS(0X0E00);
    // STA_MEMS.odata[2] = LOS_SPI_MEMS(0X0E00);
    LOS_SPI_MEMS(0X0E00);
    STA_MEMS.odata[2] = LOS_SPI_MEMS(0X0E00);
    buf = STA_MEMS.odata[2];
    STA_MEMS.gyro_y = buf*0.005*3.1415926/180.0;
}

void LOS_MEMS_Getaccx()
{
    float buf = 0;
    // STA_MEMS.odata[3] = LOS_SPI_MEMS(0X1200);
    // STA_MEMS.odata[3] = LOS_SPI_MEMS(0X1200);
    // STA_MEMS.odata[3] = LOS_SPI_MEMS(0X1200);
    LOS_SPI_MEMS(0X1200);
    STA_MEMS.odata[3] = LOS_SPI_MEMS(0X1200);
    buf = STA_MEMS.odata[3];
    STA_MEMS.acc_x = buf*0.25*9.8/1000;
}

void LOS_MEMS_Getaccy()
{
    float buf = 0;
    // STA_MEMS.odata[4] = LOS_SPI_MEMS(0X1600);
    // STA_MEMS.odata[4] = LOS_SPI_MEMS(0X1600);
    // STA_MEMS.odata[4] = LOS_SPI_MEMS(0X1600);
    LOS_SPI_MEMS(0X1600);
    STA_MEMS.odata[4] = LOS_SPI_MEMS(0X1600);
    buf = STA_MEMS.odata[4];
    STA_MEMS.acc_y = buf*0.25*9.8/1000;
}

void LOS_MEMS_Getaccz()
{
    float buf = 0;
    // STA_MEMS.odata[5] = LOS_SPI_MEMS(0X1A00);
    // STA_MEMS.odata[5] = LOS_SPI_MEMS(0X1A00);
    // STA_MEMS.odata[5] = LOS_SPI_MEMS(0X1A00);
    LOS_SPI_MEMS(0X1A00);
    STA_MEMS.odata[5] = LOS_SPI_MEMS(0X1A00);
    buf = STA_MEMS.odata[5];
    STA_MEMS.acc_y = buf*0.25*9.8/1000;
}

void LOS_MEMS_PWROn()
{
    STA_MEMS.CMD = 1;
    LOS_EPS_Switch(20, SWITCH_ON);
}

void LOS_MEMS_PWROff()
{
    STA_MEMS.CMD = 0;
    LOS_EPS_Switch(20, SWITCH_OFF);
}







